import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node


def generate_launch_description():

    robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.231')
    add_gripper = LaunchConfiguration('add_gripper', default='True')
    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default='True')
    xarm_launch_file = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('xarm_api'), 'launch'),
        '/xarm7_driver.launch.py']),
        launch_arguments = {
            'robot_ip': robot_ip,
        }.items(),
    )
    
    
    xarm_rviz_launch_file = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('xarm_description'), 'launch'),
        '/xarm7_rviz_display.launch.py']),
        launch_arguments = {
            'robot_ip': robot_ip,
            'add_gripper': add_gripper,
            'add_vacuum_gripper': add_vacuum_gripper,
        }.items(),
    )
    
    orbbec_driver_launch_file = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('orbbec_camera'), 'launch'),
        '/gemini_330_series.launch.py']),
    )
    
    aruco_single_launch_file = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('aruco_ros'), 'launch'),
        '/single.launch.py']),
    )
     
     
    gripper_control_node = Node(
        package="xarm_teleop_control",
        executable="gripper_control_node",
        name="gripper_control_node",
        output="screen"
    )
    
    xarm_teleop_control_node = Node(
        package="xarm_teleop_control",
        executable="xarm_teleop_control_node",
        name="xarm_teleop_control_node",
        output="screen"
    )    

    return LaunchDescription([
        xarm_launch_file,
        xarm_rviz_launch_file,
        orbbec_driver_launch_file,
        aruco_single_launch_file,
        gripper_control_node,
        xarm_teleop_control_node
    ])
